import numpy as np
from Plantdata import *
import os
#from plot import *
import control as ct
from NDI import *
from utils import *
os.chdir(os.path.dirname(__file__))

longdata,ladata=read_data(r'data/longdata.csv',r'data/latdata.csv')
def get_none_standard(machset,basicdata,longdata,ladata):
    for mach in machset:
        A_long,B_long,C_long,D_long,A_la,B_la,C_la,D_la,n_a=get_ABCD(longdata,ladata,basicdata,mach=mach)
##开环系统
        #longdata['n_a']
        unct_System_long=ct.ss(A_long,B_long,C_long,D_long,inputs=["de"],outputs=["v","alpha","q","theta"],states=["v","alpha","q","theta"])
        unct_System_la=ct.ss(A_la,B_la,C_la,D_la,inputs=["da","dr"],outputs=["beta","p","r","phi"],states=["beta","p","r","phi"])

#unct_System_la_tf=ct.ss2tf(unct_System_la,name='unct_la',inputs=['da','dr'],outputs=['beta','p','r','phi'])
#unct_System_long_tf=ct.ss2tf(unct_System_long,name='unct_long',inputs=['de'],outputs=['v','alpha','q','theta'])
        print("mach:",mach)
        wn,zeta,poles=ct.damp(unct_System_long,doprint=True)
        alpha_wn=wn[1]
        alpha_dmp=zeta[1]
#zerossimp,polessim=ct.pzmap(ct.balred(unct_System_long,2),plot=True)
        print("y:",(alpha_wn**2)/n_a,"n_a:",n_a,"dmp:",alpha_dmp)
#(zerossimp,polessim)
#machset=np.linspace(0.3,1.8,)
get_none_standard([0.3,0.4,0.5,1.7,1.8],basicdata,longdata,ladata)        



